#include "io_control.h"
#include <shared_parameters/shared_params_interface.hpp>

#define INPUT_DEVICES_NAMESPACE "input_device"
#define OUTPUT_DEVICES_NAMESPACE "output_device"
// #define KEY_NAV_TYPE "AGVParam/agv_navigation_type"

#define LASER_NAV_TYPE 0
#define QR_NAV_TYPE 1

// 其他控制输出点名字列表....
#define IO_OUTLIEN_LAMP_NAME "outlineLamp" // 叉车探照灯名字
#define IO_WINDFAN "windFan"               // 叉车风扇
#define IO_LEFTTURN "leftTurnLight"        // 叉车左转向灯
#define IO_RIGHTTURN "rightTurnLight"      // 叉右转向灯

// 绿灯频闪服务名称
#define SRV_SET_GREEN_BLINK "/green_light_blink"

using namespace std::chrono_literals;
namespace io_module {

IoControl::IoControl(const rclcpp::NodeOptions& options):
    Node("io_module", options),
    io_interface(nullptr),
    m_canSpeaker(nullptr),  
    m_blink(nullptr), 
    m_curMarkId(nullptr),
    m_curEvent{},
    m_curAutoStatus{},
    m_ioGroup{},
    m_agvInfo{},
    m_lamp{},
    m_soundMode(SOUND_SILENT),
    turn_flag(2),
    m_outputsNewMsg{},
    m_outputsAMsg{}  
{
    initParam();
}

IoControl::~IoControl() 
{
    m_quitMutex.unlock();
    if (m_thread.joinable()) {
        m_thread.join();
    }
}

bool IoControl::init() 
{
    rosInit();
    readParam();
    io_interface = std::make_shared<IoHardInterface>(shared_from_this());
    bool init_flag = io_interface->init();
    m_canSpeaker = std::make_shared<CanSpeaker>(shared_from_this());
    m_canSpeaker->init();
    m_blink = std::make_shared<BlinkTrigger>(shared_from_this());
    m_curMarkId = std::make_shared<CDataTimeStamp<int>>(shared_from_this());

    m_quitMutex.lock();
    m_thread = std::thread([this]() {
        while (rclcpp::ok()) {
            if (m_quitMutex.try_lock_for(25ms)) {
                break;
            }
            update();
        }
    });

    RCLCPP_INFO(get_logger(), "IoControl init %s", init_flag ? "ok" : "failed");
    return init_flag;
}

bool IoControl::initParam() 
{
    if (!SharedParamsInterface::initialize()) {
        DEBUG_ERROR_OUT("Failed to initialize shared memory interface");
        return false;
    }

    try {
        auto params = SharedParamsInterface::getNodeParams("shared_parameters");
        if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
          DEBUG_ERROR_OUT("Error get shared parameters");
          return false;
        } 

        params = SharedParamsInterface::getNodeParams("IO");
        for (const auto& [name, value] : params) {
           
            RCLCPP_INFO_STREAM(get_logger(), 
                    "param:" << name << ", value:" << value.toString());
            switch(value.type) {
                case SharedParamsInterface::ParamValue::Type::INT:
                    this->declare_parameter(name, value.getValue<int>());
                    break;
                case SharedParamsInterface::ParamValue::Type::DOUBLE:
                    this->declare_parameter(name, value.getValue<double>());
                    break;
                case SharedParamsInterface::ParamValue::Type::STRING:
                    this->declare_parameter(name, value.getValue<std::string>());
                    break;
                case SharedParamsInterface::ParamValue::Type::BOOL:
                    this->declare_parameter(name, value.getValue<bool>());
                    break;
                default:
                    RCLCPP_WARN_STREAM(get_logger(), 
                            "Unsupported parameter type for " << name);
                    break;
            }
        }
    } catch (const std::exception& e) {
        DEBUG_ERROR_OUT("Error reading parameters: " << e.what());
        return false;
    }
    return true;
}

void IoControl::rosInit() 
{
    m_outputServer = this->create_service<agv_srvs::srv::OutputInterface>(
        "/outputinterface",
        [this](const std::shared_ptr<agv_srvs::srv::OutputInterface::Request> req,
               std::shared_ptr<agv_srvs::srv::OutputInterface::Response> resp) {
            this->outputSrvCallBack(req, resp);
        });

    m_outputServerNew = this->create_service<agv_srvs::srv::OutputsNew>(
        "/outputNew",
        [this](const std::shared_ptr<agv_srvs::srv::OutputsNew::Request> req,
               std::shared_ptr<agv_srvs::srv::OutputsNew::Response> resp) {
            this->outputSrvNewCallBack(req, resp);
        });

    m_srv_green_blink = this->create_service<std_srvs::srv::Trigger>(
        SRV_SET_GREEN_BLINK,
        [this](const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
               std::shared_ptr<std_srvs::srv::Trigger::Response> resp) {
            this->handleGreenBlink(req, resp);
        });

    m_outputAServer = this->create_service<agv_srvs::srv::OutputsAnalog>(
        "/outputAnalog",
        [this](const std::shared_ptr<agv_srvs::srv::OutputsAnalog::Request> req,
               std::shared_ptr<agv_srvs::srv::OutputsAnalog::Response> resp) {
            this->outputASrvCallBack(req, resp);
        });

    // 初始化订阅者
    m_agvinfoSub = this->create_subscription<agv_msgs::msg::AGVMode>(
        "/agvmode", 10,
        [this](const agv_msgs::msg::AGVMode::SharedPtr msg) {
            this->agvinfoCallBack(msg);
        });

    m_eventSub = this->create_subscription<agv_msgs::msg::AGVEventStatus>(
        "/eventstatus", 10,
        [this](const agv_msgs::msg::AGVEventStatus::SharedPtr msg) {
            this->eventCallBack(msg);
        });

    m_autoRunSub = this->create_subscription<agv_msgs::msg::AgvAutoStatus>(
        "/auto_run_status", 2,
        [this](const agv_msgs::msg::AgvAutoStatus::SharedPtr msg) {
            this->autoRunStatusCallBack(msg);
        });

    m_landMarkSub = this->create_subscription<geometry_msgs::msg::PoseStamped>(
        "/landmark", 2,
        [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
            this->getLandMarkCB(msg);
        });

    m_setOutputSub = this->create_subscription<agv_msgs::msg::OutPutsNew>(
        "/setOutput", 10,
        [this](const agv_msgs::msg::OutPutsNew::SharedPtr msg) {
            this->setOutputCallback(msg);
        });

    m_odomSub = this->create_subscription<agv_msgs::msg::OdometryLite>(
        "/odom", 2,
        [this](const agv_msgs::msg::OdometryLite::SharedPtr msg) {
            this->odomCallBack(msg);
        });

    
    m_inputPub = this->create_publisher<agv_msgs::msg::ReadInPuts>("/readInput", 10);
    m_rawInputPub = this->create_publisher<agv_msgs::msg::ReadInPuts>("/readInputRaw", 2);
    m_inputNewPub = this->create_publisher<agv_msgs::msg::ReadInPutsNew>("/readInputNew", 10);
    m_outputNewPub = this->create_publisher<agv_msgs::msg::OutPutsNew>("/outputNew", 5);
    m_inputAPub = this->create_publisher<agv_msgs::msg::ReadInPutsAnalog>("/readInputAnalog", 10);
    m_outputAPub = this->create_publisher<agv_msgs::msg::OutputsAnalog>("/outputAnalog", 5);

    DEBUG_OUT("ROS interfaces initialization completed");
}

void IoControl::readParam() 
{
    // 读取电平极性参数
    readPolarityParam();

    // 新增读取所有的io输入设备
    readInOutParam();

    // 根据输出配置列表-更新输出io状态表字段
    initOutputsNewMsg();

    // 根据配置的输出通道列表初始化topic消息字段
    initOutputsAMsg();
}

void IoControl::initOutputsAMsg()
{
    // 输出通道数量
    auto size = m_ioGroup.analog_output_device.size();
    m_outputsAMsg.array.clear();
    m_outputsAMsg.array.resize(size);
    // 初始化通道索引
    for (std::size_t i = 0; i < m_outputsAMsg.array.size(); i++)
    {
        auto &data = m_outputsAMsg.array[i];
        data.channel = i;
        data.value = 0;
    }
}

void IoControl::initOutputsNewMsg()
{
    m_outputsNewMsg.array.clear();
    for (auto &device : new_output_devices)
    {
        agv_msgs::msg::PairTypeInt data;
        data.key = device.first;
        data.value = 0;
        m_outputsNewMsg.array.push_back(data);
    }
}

// 新增以后扩展使用，发送(key字符串-输入io名字):int(level)
void IoControl::readInOutParam()
{
    // 读取参数列表内的输入IO设备
    new_input_devices.clear();
    new_output_devices.clear();

    /* rewrite
    ros::NodeHandle nh;
    std::vector<std::string> param_names;
    nh.getParamNames(param_names); // 获取该命名空间下的所有参数名称

    for (auto param : param_names)
    {
        std::size_t pos_in = param.find(INPUT_DEVICES_NAMESPACE);
        std::size_t pos_out = param.find(OUTPUT_DEVICES_NAMESPACE);

        if (pos_in != std::string::npos)
        {
            int ioIndex = -1;
            nh.param<int>(param, ioIndex, -1);
            if (ioIndex < 0 || ioIndex >= (int)(m_ioGroup.polarity_input.size()))
            {
                ROS_ERROR_STREAM("error:readInParam:current param.name=" << param << ",ioIndex=" << ioIndex);
                continue;
            }

            std::string key = param.erase(pos_in, std::string(INPUT_DEVICES_NAMESPACE).length());
            new_input_devices.push_back({key, ioIndex});

            DEBUG_STREAM_OUT(
                "Found at input pos: " << pos_in << "param.key=" << key << ",ioIndex=" << ioIndex);
        }
        else if (pos_out != std::string::npos)
        {
            int ioIndex = -1;
            nh.param<int>(param, ioIndex, -1);
            if (ioIndex < 0 || ioIndex >= (int)(m_ioGroup.polarity_output.size()))
            {
                ROS_ERROR_STREAM("error:readOutParam:current param.name=" << param << ",ioIndex=" << ioIndex);
                continue;
            }
            std::string key = param.erase(pos_out, std::string(OUTPUT_DEVICES_NAMESPACE).length());
            new_out_devices.push_back({key, ioIndex});

            DEBUG_STREAM_OUT(
                "Found at output pos: " << pos_out << "param.key=" << key << ",ioIndex=" << ioIndex);
        }
        else
        {
            // std::cout << "Not found" << std::endl;
            continue;
        }
    }*/
    
    initIODevice(new_input_devices, INPUT_DEVICES_NAMESPACE);
    initIODevice(new_output_devices, OUTPUT_DEVICES_NAMESPACE);

}

void IoControl::readPolarityParam()
{
    // 读取输入输出点的数量
    this->get_parameter_or("num_digital_input", m_ioGroup.num_input, 0);
    this->get_parameter_or("num_digital_output", m_ioGroup.num_output, 0);
    // 读取模拟量通道输入输出的数量
    this->get_parameter_or("num_analog_input", m_ioGroup.num_analog_input, 0);
    this->get_parameter_or("num_analog_output", m_ioGroup.num_analog_output, 0);

    // 调整io储存大小
    m_ioGroup.resize();

    // 读取输入点的极性 索引从1起
    bool polarity;
    for (int i = 0; i < m_ioGroup.num_input; i++)
    {
        this->get_parameter_or("polarity_input/polarity_input_" + std::to_string(i + 1), polarity, false);
        m_ioGroup.polarity_input[i] = polarity;
        DEBUG_STREAM_OUT("polarity_input_" << (i + 1) << "=" << (int)m_ioGroup.polarity_input[i]);
    }

    // 读取输出点的极性 索引从1起
    for (int i = 0; i < m_ioGroup.num_output; i++)
    {
        this->get_parameter_or("polarity_output/polarity_output_" + std::to_string(i + 1), polarity, false);
        m_ioGroup.polarity_output[i] = polarity;
        DEBUG_STREAM_OUT("polarity_output_" << (i + 1) << "=" << (int)m_ioGroup.polarity_output[i]);
    }

    // 读取输出设备-声光设备 更新迭代器内数据
    for (auto iter = light_sound_devices.begin(); iter != light_sound_devices.end(); iter++)
    {
        this->get_parameter_or("output_device/" + iter->first, iter->second, -1);
        DEBUG_STREAM_OUT("light_sound: output device:" << (iter->first) << ",index:" << iter->second);
    }

    //  兼容旧的接口，暂时保留
    //  读取输入设备
    //  旧的bit位的输入
    // for (std::size_t i = 0; i < input_devices.size(); i++)
    // {
    //     m_priNh.param<int>(("/IO/input_device/" + input_devices[i].first), input_devices[i].second, -1);
    //     DEBUG_STREAM_OUT("input device:" << input_devices[i].first << ",index:" << input_devices[i].second);
    // }

    // 读取模拟量参数
    readAnalogParam();
}

// 读取模拟量参数
void IoControl::readAnalogParam()
{
    // 读取输入通道的参数 索引从1起
    std::string in_prefix = "analog_input_device/analog_input_";
    std::string out_prefix = "analog_output_device/analog_output_";
    for (int i = 0; i < m_ioGroup.num_analog_input; i++)
    {
        AnalogChannel channel;
        auto prefix = in_prefix + std::to_string(i + 1) + "/";
        this->get_parameter_or(prefix + "norm_min", channel.norm_min, 0.0);
        this->get_parameter_or(prefix + "norm_max", channel.norm_max, 0.0);
        this->get_parameter_or(prefix + "raw_data_min", channel.raw_data_min, 0);
        this->get_parameter_or(prefix + "raw_data_max", channel.raw_data_max, 0);

        m_ioGroup.analog_input_device[i] = channel;
        DEBUG_STREAM_OUT("analog_input_" << (i + 1) << ":"
                                        << "norm_min=" << channel.norm_min
                                        << ",norm_max=" << channel.norm_max
                                        << ",raw_data_min=" << channel.raw_data_min
                                        << ",raw_data_max=" << channel.raw_data_max);
    }

    // 读取输出通道的参数 索引从1起
    for (int i = 0; i < m_ioGroup.num_analog_output; i++)
    {
        AnalogChannel channel;
        auto prefix = out_prefix + std::to_string(i + 1) + "/";
        this->get_parameter_or(prefix + "norm_min", channel.norm_min, 0.0);
        this->get_parameter_or(prefix + "norm_max", channel.norm_max, 0.0);
        this->get_parameter_or(prefix + "raw_data_min", channel.raw_data_min, 0);
        this->get_parameter_or(prefix + "raw_data_max", channel.raw_data_max, 0);

        m_ioGroup.analog_output_device[i] = channel;
        DEBUG_STREAM_OUT("analog_output_" << (i + 1) << ":"
                                         << "norm_min=" << channel.norm_min
                                         << ",norm_max=" << channel.norm_max
                                         << ",raw_data_min=" << channel.raw_data_min
                                         << ",raw_data_max=" << channel.raw_data_max);
    }
}

void IoControl::initIODevice(std::map<std::string, std::vector<DEVICE_TYPE>>& devices, 
                           const std::string& param_path) {
    const auto params = this->list_parameters({}, 10);
    DEBUG_STREAM_OUT("Parameters under path '" << param_path << "':");

    for (const auto& name : params.names) {
        if (name.find(param_path) != 0) continue;
        
        std::string short_name = name.substr(param_path.length());
        if (!short_name.empty() && short_name[0] == '/') {
            short_name.erase(0, 1); 
        }

        size_t last_slash = short_name.find_last_of('/');  
        if (last_slash == std::string::npos) {
            int value;
            if (this->get_parameter(name, value)) {
                devices[short_name].emplace_back("", value);  // group设为空
                DEBUG_OUT("initIODevice devices add [%s] [] [%d]", 
                        short_name.c_str(), value);
            }
        } else {
            std::string group = short_name.substr(0, last_slash);
            std::string member = short_name.substr(last_slash + 1);
            
            int value;
            if (this->get_parameter(name, value)) {
                devices[group].emplace_back(member, value);
                DEBUG_OUT("initIODevice devices add [%s] [%s] [%d]", 
                        group.c_str(), member.c_str(), value);
            }
        }
    }
}

void IoControl::agvinfoCallBack(const agv_msgs::msg::AGVMode::SharedPtr& msg)
{
    // 此处除了暂时使用AGV手自动模式等，报警事件灯光闪烁，采用event_occur来报警
    uint64_t mode = msg->mode;
    m_agvInfo.manul = mode == agv_msgs::msg::AGVMode::MANUAL;
    m_agvInfo.halfAuto = mode == agv_msgs::msg::AGVMode::SEMI_AUTOMATIC;
    m_agvInfo.autoOnline = mode == agv_msgs::msg::AGVMode::AUTOMATIC;
    m_agvInfo.init = true;
}

void IoControl::odomCallBack(const agv_msgs::msg::OdometryLite::SharedPtr& msg)
{
    if (msg->twist.angular.z > 0.02)
    {
        turn_flag = 0; // 左转
    }
    else if (msg->twist.angular.z < -0.02)
    {
        turn_flag = 1; // 右转
    }
    else
    {
        turn_flag = 2; // 灭
    }
}


//
// 服务端：设置一次多路IO输出(名字控制方式:不带频率)
//
bool IoControl::outputSrvNewCallBack(const std::shared_ptr<agv_srvs::srv::OutputsNew::Request> req,
                            std::shared_ptr<agv_srvs::srv::OutputsNew::Response> resp)
{

    std::vector<agv_msgs::msg::OutputPort> outs;
    // 需要输出的io设备名字列表
    IO_GROUP devices_set = req->array;
    for (auto device_set : devices_set)
    {
        /* rewrite
        agv_msgs::OutputPort out;
        bool find = false;
        // 参数列表内配置好的输出设备，查找有此配置的名称
        for (auto io_of_list : new_out_devices)
        {
            // 找到列表内相同的名字：匹配成功
            if (device_set.key == io_of_list.first)
            {
                // 列表配置的io索引
                if (io_of_list.second < 0 || io_of_list.second >= (int)(m_ioGroup.polarity_output.size()))
                {
                    // 未配置，则返回失败
                    resp.state = false;
                    DEBUG_STREAM_OUT("#1 outputSrvNewCallBack failed:io index is -1,name=" << device_set.key);
                    return false;
                }
                out.index = io_of_list.second;
                out.level = device_set.value;
                outs.push_back(out);
                find = true;
                break;
            }
        }
        
        if (!find)
        {
            // 列表内没有找到此字段；返回失败
            resp.state = false;
            DEBUG_STREAM_OUT("#2 outputSrvNewCallBack failed:can not find io name=" << device_set.key);
            return false;
        }
        */

        if (new_output_devices.find(device_set.key) != new_output_devices.end()) {
            bool available = false;
            for (const auto& member : new_output_devices[device_set.key]) {
                if (member.second >= 0 && member.second < (int)(m_ioGroup.polarity_output.size())) {
                    available = true;
                    agv_msgs::msg::OutputPort out;
                    out.index = member.second;
                    out.level = device_set.value;
                    outs.push_back(out);
                }
            }

            if (!available) {
                resp->state = false;
                DEBUG_STREAM_OUT("#1 outputSrvNewCallBack failed:io index is -1,name=" << device_set.key);
                return false;
            }
        }
        else {
            resp->state = false;
            DEBUG_STREAM_OUT("#2 outputSrvNewCallBack failed:can not find io name=" << device_set.key);
            return false;
        }        
    }

    resp->state = setOutputs(outs);
    if (!resp->state)
    {
        DEBUG_STREAM_OUT("outputSrvCallBack failed");
    }
    else
    {
        DEBUG_OUT("outputSrvNewCallBack OK");
        recordOutputs(devices_set);
    }

    return true; // resp.state;
}


// 置出通道输出-需转换为原始数据(norm->raw_data)
bool IoControl::setOutputsA(std::vector<agv_msgs::msg::PairTypeAnalog> outsA)
{
    bool ret = false;
    // 查看模拟量输出通道的列表
    for (auto p : outsA)
    {
        auto channel = p.channel;
        auto value = p.value;
        if (channel < 0 || channel >= (int)(m_ioGroup.analog_output_device.size()))
        {
            DEBUG_ERROR_OUT("error:setOutputsA.channel=" << channel << "  is out range");
            ret = false;
            break;
        }
        // AnalogChannel
        auto cur = m_ioGroup.analog_output_device[channel];

        // 根据输入极性，判断是否取反逻辑电平
        uint16_t rawData = 0;
        if (cur.toRawData(value, rawData))
        {
            ret = io_interface->set_outputsA(channel, rawData);
        }

        DEBUG_STREAM_OUT("channel:" << channel
                                   << ",norm=" << value << ",toRaw=" << rawData
                                   << ",ret=" << (int)(ret));
        if (!ret)
        {
            DEBUG_ERROR_OUT("error: out.channel=" << channel << ",norm=" << value << ",toRaw=" << rawData << "  set_outputsA failed");
            break;
        }
    }

    return ret;
}

//
// 服务端：设置一次多路模拟量输出
//
bool IoControl::outputASrvCallBack(const std::shared_ptr<agv_srvs::srv::OutputsAnalog::Request> req,
                           std::shared_ptr<agv_srvs::srv::OutputsAnalog::Response> resp)
{
    // 需要输出的通道索引名字列表
    auto outsA = req->array;
    resp->state = setOutputsA(outsA);
    if (!resp->state)
    {
        DEBUG_STREAM_OUT("outputASrvNewCallBack failed");
    }
    else
    {
        DEBUG_STREAM_OUT("outputASrvNewCallBack OK");
        recordOutputsA(outsA);
    }

    return true; // resp.state;
}



// 置出指定io点输出 支持输出多个
bool IoControl::setOutputs(std::vector<agv_msgs::msg::OutputPort> outports)
{
    bool ret = false;

    // 设置无频率
    SetFrequency out;
    out.clear();
    // 查看输出的IO列表
    for (auto p : outports)
    {
        if (p.index < 0 || p.index >= (int)(m_ioGroup.polarity_output.size()))
        {
            DEBUG_ERROR_OUT("error:setout.index=" << p.index << "  is out range");
            ret = false;
            break;
        }

        // 根据输入极性，判断是否取反逻辑电平
        out.index = p.index;
        out.level = m_ioGroup.polarity_output[out.index] ? p.level : !p.level;

        // 输出io
        ret = io_interface->set_interval_reg(out);
        DEBUG_STREAM_OUT("index:" << out.index << ",level=" << out.level << ",ret=" << (int)(ret));
        if (!ret)
        {
            DEBUG_ERROR_OUT("out.index=" << out.index << "  set_interval_reg failed");
            break;
        }
    }

    return ret;
}

//  landmark的回调, 在扫到二维码才有数据, 才会进入回调
void IoControl::getLandMarkCB(const geometry_msgs::msg::PoseStamped::SharedPtr& mark_data)
{
    geometry_msgs::msg::Pose realPose = mark_data->pose; // 二维码计算出来相对车体中心的位姿数据（暂时不需要）
    m_curMarkId->update(realPose.position.z);       // 二维码读取出来的ID
}

void IoControl::setOutputCallback(const agv_msgs::msg::OutPutsNew::SharedPtr& msg)
{
    std::vector<agv_msgs::msg::OutputPort> outs;
    IO_GROUP devices_set = msg->array;
    for (auto device_set : devices_set)
    {
        if (new_output_devices.find(device_set.key) != new_output_devices.end()) {
            bool available = false;
            for (const auto& member : new_output_devices[device_set.key]) {
                if (member.second >= 0 && member.second < (int)(m_ioGroup.polarity_output.size())) {
                    available = true;
                    agv_msgs::msg::OutputPort out;
                    out.index = member.second;
                    out.level = device_set.value;
                    outs.push_back(out);
                }
            }

            if (!available) {
                DEBUG_ERROR_OUT("#1 setOutputCallback failed:io index is -1,name=" << device_set.key);
            }
        }
        else {
            DEBUG_ERROR_OUT("#2 setOutputCallback failed:can not find io name=" << device_set.key);
        }        
    }

    setOutputs(outs);
}

bool IoControl::outputSrvCallBack(const std::shared_ptr<agv_srvs::srv::OutputInterface::Request> req,
                                 std::shared_ptr<agv_srvs::srv::OutputInterface::Response> resp) 
{
    std::vector<agv_msgs::msg::OutputPort> ports;
    for (auto& item : req->index_level) {
        agv_msgs::msg::OutputPort port;
        port.index = item.index;
        port.level = item.level;
        ports.push_back(port);
    }
    resp->state = setOutputs(ports);
    return true;
}

void IoControl::threadFun() 
{
    while (rclcpp::ok()) {
        if (m_quitMutex.try_lock_for(25ms)) {
            break;
        }
        update();
    }
}

void IoControl::pubReadInputs() 
{
    // #1 原始io输入数据
    uint64_t inputs_raw = 0;
    // #2 反转极性之后的io输入数据
    uint64_t inputs_later = 0;
    bool ret = io_interface->read_input_ios(m_ioGroup.num_input, inputs_raw);

    if (!ret)
    {
        DEBUG_ERROR_OUT("read input Io failed");
        return;
    }

    // #3 反转原始数据的输入IO的极性
    inputs_later = inputs_raw;
    // 数字输入io的数量(默认16路)
    for (int i = 0; i < m_ioGroup.num_input; i++)
    {
        // 如果是极性反逻辑，则只修改当前位
        if (m_ioGroup.polarity_input[i] == 0)
        {
            inputs_later = NEGATION_BIT(inputs_later, i);
        }
    }

    // #4  64位定义、固定的抽象io对象顺序 例如：第一位：stopButton 停止按钮 第二位：resetButton 复位按钮 ... add at 2022-8-19
    uint64_t tartget_device_bits = 0;
    for (std::size_t i = 0; i < input_devices.size(); i++)
    {
        // 硬件io的具体定义，如果未设置则为-1 跳过检测
        DEVICE_TYPE curDeviceObj = input_devices[i];
        int curIndex = curDeviceObj.second;
        if (curIndex < 0)
        {
            continue;
        }
        // 查找当前硬件io对象的对应是第几个IO输入点
        bool curLevelBit = TestBit(inputs_later, curIndex);
        // 如果输入点是逻辑高，则置位(触发)，逻辑低，则清位(未触发)
        tartget_device_bits = curLevelBit ? SET_BIT(tartget_device_bits, i) : CLEAR_BIT(tartget_device_bits, i);
    }
    // 设置readInput话题发布
    agv_msgs::msg::ReadInPuts input;
    input.ret_bits = tartget_device_bits;
    m_inputPub->publish(input);

    // #5 发布原始输入io消息格式(参数逻辑电平反转之后的数据)
    agv_msgs::msg::ReadInPuts inputRaw;
    inputRaw.ret_bits = inputs_later;
    m_rawInputPub->publish(inputRaw);

    // #6  设置readInput话题发布
    agv_msgs::msg::ReadInPutsNew inputsNew;
    for (const auto& group : new_input_devices) {
        int value = 0;
        for (const auto& member : group.second) {
            value |= (member.second < 0 ? 0 : TestBit(inputs_later, member.second));
        }        

        agv_msgs::msg::PairTypeInt pairType;
        pairType.key = group.first;
        pairType.value = value;
        inputsNew.array.push_back(pairType);
    }
    m_inputNewPub->publish(inputsNew);

    /* rewrite
    for (std::size_t i = 0; i < new_input_devices.size(); i++)
    {
        // 硬件io的具体定义，如果未设置则为-1 跳过检测
        DEVICE_TYPE curDeviceObj = new_input_devices[i];
        int curIndex = curDeviceObj.second;
        agv_msgs::PairTypeInt pairType;
        pairType.key = curDeviceObj.first;
        pairType.value = 0;
        if (curIndex < 0)
        {
            inputsNew.array.push_back(pairType);
            continue;
        }
        // 查找当前硬件io对象的对应是第几个IO输入点
        pairType.value = TestBit(inputs_later, curIndex);
        // 放入消息列表内
        inputsNew.array.push_back(pairType);
    }
    m_inputNewPub->publish(inputsNew);
    */
}

void IoControl::pubReadInputsA()
{
    // 一次性读取一次整个模块的输入通道的状态
    // 模拟量原始数据输入数据
    std::vector<BaseInterface::WORD> inputsA_raw;
    inputsA_raw.clear();
    agv_msgs::msg::ReadInPutsAnalog inputsA_norm;
    inputsA_norm.array.clear();
    auto size = m_ioGroup.num_analog_input;
    bool ret = io_interface->read_inputsA(size, inputsA_raw);
    bool same = size == (int)(inputsA_raw.size());
    if (!ret || !same)
    {
        DEBUG_ERROR_OUT("read input analog failed;ret=" << ret << ",size is same=" << same);
        return;
    }

    // 归一化数据再发布
    for (int i = 0; i < size; i++)
    {
        agv_msgs::msg::PairTypeAnalog pair;
        auto curChannel = m_ioGroup.analog_input_device[i];
        auto curRaw = inputsA_raw[i];
        double norm = 0.0f;
        pair.channel = i;
        if (curChannel.toNorm(curRaw, norm))
        {
            pair.value = norm;
            inputsA_norm.array.push_back(pair);
        }
        else
        {
            DEBUG_ERROR_OUT("error: pubReadInputsA toNorm failed;i=" << i << ",curRaw=" << curRaw);
            return;
        }
    }

    inputsA_norm.header.stamp = this->now();
    // 设置话题发布
    m_inputAPub->publish(inputsA_norm);
}

// 记录输出状态
void IoControl::recordOutputs(IO_GROUP outputs)
{
    for (auto &updateData : outputs)
    {
        bool find = false;
        for (auto &data : m_outputsNewMsg.array)
        {
            if (updateData.key == data.key)
            {
                data.value = updateData.value;
                find = true;
                break;
            }
        }

        if (!find)
        {
            DEBUG_ERROR_OUT("error:recordOutputs failed ;cannot find outIo key=" << updateData.key << ",value=" << updateData.value);
        }
    }
}

void IoControl::recordOutputsA(std::vector<agv_msgs::msg::PairTypeAnalog> outsA)
{

    for (auto &updateData : outsA)
    {
        bool find = false;
        for (auto &data : m_outputsAMsg.array)
        {
            if (updateData.channel == data.channel)
            {
                data.value = updateData.value;
                find = true;
                break;
            }
        }

        if (!find)
        {
            DEBUG_ERROR_OUT("error:recordOutputsA failed ;cannot find  channel=" << updateData.channel << ",value=" << updateData.value);
        }
    }
}

// 发布输出io的话题-只记录通过/outputNew服务接口输出的IO记录
void IoControl::pubOutPutsNew()
{
    m_outputNewPub->publish(m_outputsNewMsg);
}

// 发布输出io的话题-只记录通过/outputNew服务接口输出的IO记录
void IoControl::pubOutPutsA()
{
    m_outputAPub->publish(m_outputsAMsg);
}

bool IoControl::update()
{
    // 发布输入IO话题
    pubReadInputs();
    // 发布输出IO话题
    pubOutPutsNew();
    // 发布模拟量输入话题
    pubReadInputsA();
    // 发布模拟量输出状态记录话题
    pubOutPutsA();
    // 根据事件和模式使用场景更新声光报警
    // updateSoundLightByScene();

    // 特殊io点输出
    // specialIoOut();

    return true;
}

void IoControl::eventCallBack(const agv_msgs::msg::AGVEventStatus::SharedPtr& event)
{
    m_curEvent = *event;
}

void IoControl::autoRunStatusCallBack(const agv_msgs::msg::AgvAutoStatus::SharedPtr& status)
{
    m_curAutoStatus = *status;
}

int IoControl::getNavType()
{
    int nNavType = -1;
    nNavType = SharedParamsInterface::getParam("AGVParam", "agv_navigation_type", 0);
    return nNavType;
}

// 如果是二维码模式且有二维码
bool IoControl::findQr()
{
    bool find = false;
    bool qrNav = getNavType() == QR_NAV_TYPE;
    bool idOk = m_curMarkId->is_new(300) && m_curMarkId->get() > 0;
    find = qrNav && idOk;
    return find;
}

bool IoControl::handleGreenBlink(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
               std::shared_ptr<std_srvs::srv::Trigger::Response> resp)
{
    DEBUG_OUT("handleGreenBlink once");
    resp->success = true;
    resp->message = "ready to set green blink";
    m_blink->triggerOnce();
    return true;
}

void IoControl::setRgyLamp(bool green, bool yellow, bool red, bool isblink, int on, int off)
{
    m_lamp.color["green"] = green;
    m_lamp.color["yellow"] = yellow;
    m_lamp.color["red"] = red;
    m_lamp.on_time_ = on;
    m_lamp.off_time_ = off;
    if (isblink == true)
    {
        // 带频率，电平会自动忽略
        m_lamp.set_frequency_ = true;
        m_lamp.level_ = false;
    }
    else
    {
        // 不带频率，电平设置有效
        m_lamp.set_frequency_ = false;
        m_lamp.level_ = true;
    }
}

void IoControl::updateSoundLightByScene()
{
    // 更新特殊场景-自动运行的状态
    // m_specialScene.updateStatus(m_curAutoStatus);
    // int sceneIndex = m_specialScene.getScene();

    // 清除三色灯变量
    m_lamp.cur_scene = SceneName0;
    setRgyLamp(false, false, false, false, 0, 0);
    m_soundMode = SOUND_SILENT;

    // #故障触发，红灯闪烁
    if (m_curEvent.ecode != agv_msgs::msg::AGVEvent::NONE_EVENT)
    {
        switch (m_curEvent.ecode)
        {
        case agv_msgs::msg::AGVEvent::SAFE_DETECT_NEAR_EVENT:
        {
            // 避障:红灯闪烁 500ms + 500ms
            // 避障声
            m_lamp.cur_scene = SceneName1;
            setRgyLamp(false, false, true, true, 500, 500);
            m_soundMode = SOUND_OBSTACLE;
        }
        break;
        case agv_msgs::msg::AGVEvent::SAFE_DETECT_CENTER_EVENT:
        case agv_msgs::msg::AGVEvent::SAFE_DETECT_FAR_EVENT:
        {
            // 避障:红灯闪烁 500ms + 500ms
            // 避障声
            m_lamp.cur_scene = SceneName2;
            setRgyLamp(false, true, false, true, 500, 500);
            m_soundMode = SOUND_OBSTACLE;
        }
        break;
        // case agv_msgs::msg::AGVEvent::TASK_ERROR_EVENT:
        // {
        //     // 任务错误:黄灯闪烁 500ms + 500ms
        //     // 运行声
        //     m_lamp.cur_scene = SceneName3;
        //     setRgyLamp(false, true, false, true, 500, 500);
        //     m_soundMode = SOUND_RUN;
        // }
        // break;
        default:
        {
            // 其他故障:红灯闪烁 500ms + 500ms
            // 报警声
            m_lamp.cur_scene = SceneName4;
            setRgyLamp(false, false, true, true, 500, 500);
            m_soundMode = SOUND_ALARM;
        }
        break;
        }
    }
    // 手动模式 绿灯闪烁 800ms+800ms
    // 声音静默
    else if (m_agvInfo.manul)
    {
        if (!findQr())
        {
            m_lamp.cur_scene = SceneName5;
            setRgyLamp(true, false, false, true, 800, 800);
            m_soundMode = SOUND_SILENT;
        }
        else
        {
            m_lamp.cur_scene = SceneName12;
            setRgyLamp(false, true, false, true, 800, 800);
            m_soundMode = SOUND_SILENT;
        }
    }
    // 半自动模式 黄灯闪烁 1000ms+1000ms
    // 运行音
    else if (m_agvInfo.halfAuto)
    {
        m_lamp.cur_scene = SceneName6;
        setRgyLamp(false, true, false, true, 1000, 1000);
        m_soundMode = SOUND_RUN;
    } // 自动模式
    else
    {
        // 声音设置
        bool back_move = backward();
        if (m_blink->trigState())
        {
            // 强制设置绿灯闪烁-触发放行动作
            m_lamp.cur_scene = SceneName15;
            setRgyLamp(true, false, false, true, 300, 300);
            m_soundMode = SOUND_ALARM;
        }
        else if (!navRun())
        {
            // 不是导航工作模式
            // 绿灯常亮 声音静默
            m_lamp.cur_scene = SceneName7;
            setRgyLamp(true, false, false, false, 0, 0);
            m_soundMode = SOUND_SILENT;
        }
        else if(spinRun())
        {
            // 自旋时
            // 绿灯常亮 提示自旋音报警音
            m_lamp.cur_scene = SceneName16;
            setRgyLamp(true, false, false, false, 0, 0);
            m_soundMode = SOUND_SPIN;
        }
        else if (back_move)
        {
            // 后退时
            // 绿灯常亮 提示报警音
            m_lamp.cur_scene = SceneName8;
            setRgyLamp(true, false, false, false, 0, 0);
            m_soundMode = SOUND_ALARM;
        }
        else if (opRun())
        {
            // 静态操作模式
            // 黄灯常亮 声音静默
            m_lamp.cur_scene = SceneName9;
            setRgyLamp(false, true, false, false, 0, 0);
            m_soundMode = SOUND_SILENT;
        }
        // 自动调度
        else if (m_agvInfo.autoOnline)
        {
            // 在线离线控制   （0：单机模式，1：自动master模式）绿灯长亮
            // 自动调度 绿灯常亮 运行声
            m_lamp.cur_scene = SceneName10;
            setRgyLamp(true, false, false, false, 0, 0);
            m_soundMode = SOUND_RUN;
        } // 自动本地
        else
        {
            if (m_agvInfo.init)
            {
                // 自动本地模式
                // 绿灯常亮 运行声
                m_lamp.cur_scene = SceneName11;
                setRgyLamp(true, false, false, false, 0, 0);
                m_soundMode = SOUND_RUN;
            }
        }
    }

    // 场景切换时，切换一次声光
    if (m_lamp.last_scene != m_lamp.cur_scene)
    {
        // 输出灯光和声音
        light();
        sound();
        DEBUG_STREAM_OUT("last_scene =" << m_lamp.last_scene << ",cur_scene" << m_lamp.cur_scene);
        m_lamp.last_scene = m_lamp.cur_scene;
    }
    // DEBUG_STREAM_OUT("cur_scene=" << m_lamp.cur_scene);
}

// 输出IO是否存在参数配置列表中
bool IoControl::ioExist(std::string key)
{
    /* rewrite
    auto datas = new_out_devices;
    try
    {
        auto it = std::find_if(datas.begin(), datas.end(), [key](DEVICE_TYPE &data)
                               {
                                      bool find = (key == data.first) && (data.second !=-1);
                                      return find; });
        return it != datas.end();
    }
    catch (...)
    {
        DEBUG_ERROR_OUT("ioExist crash key=" << key);
        return false;
    }

    return false;
    */

    return new_output_devices.find(key) != new_output_devices.end();
}

void IoControl::ioSpeaker()
{
    // 运行模式： 是否开启声音1-运行声
    MapIterType deviceIter = light_sound_devices.find("speaker_1");
    soundLightIoSet((m_soundMode == SOUND_RUN ? "speaker" : "close"), deviceIter);

    // 报警模式:是否开启声音2-报警声
    deviceIter = light_sound_devices.find("speaker_2");
    soundLightIoSet((m_soundMode == SOUND_ALARM || m_soundMode == SOUND_OBSTACLE ? "speaker" : "close"), deviceIter);

    // 静默模式，两种声音都是关闭状态，所以不需要再次设置
    // if (m_soundMode == SOUND_SILENT)

    DEBUG_STREAM_OUT("sound:m_soundMode=" << m_soundMode);
}

// 是否是动作或者充电等运行
bool IoControl::opRun()
{
    bool op = agv_msgs::msg::AgvAutoStatus::IS_IN_OP == m_curAutoStatus.workstate;
    bool idle = m_curAutoStatus.idle;
    return op && !idle;
}

// 是否是导航运行
bool IoControl::navRun()
{
    bool nav = agv_msgs::msg::AgvAutoStatus::IS_NAVIGATE == m_curAutoStatus.workstate;
    bool idle = m_curAutoStatus.idle;
    return nav && !idle;
}

// 是否是自旋
bool IoControl::spinRun()
{
    bool nav = agv_msgs::msg::AgvAutoStatus::IS_NAVIGATE == m_curAutoStatus.workstate;
    bool spin = m_curAutoStatus.trajtype == agv_msgs::msg::AgvAutoStatus::TRAJ_SPIN;
    return nav && spin;
}

// 是否时运行时后退
bool IoControl::backward()
{
    bool ret = false;
    int useBackAlarm = 0;
    this->get_parameter_or("use_back_alarm", useBackAlarm, 0);
    if (!useBackAlarm)
    {
        return ret;
    }

    bool back = BACKWARD == m_curAutoStatus.wheelmovedir;
    bool drive = m_curAutoStatus.driving;
    ret = back && drive && navRun();
    DEBUG_STREAM_OUT("backward=" << ret << ",back=" << back << ",drive=" << drive);
    return ret;
}


//
// 声音输出
//
void IoControl::sound()
{
    // #1 can 扬声器
    if (m_canSpeaker->enable())
    {
        DEBUG_STREAM_OUT("can:m_soundMode=" << m_soundMode);
        m_canSpeaker->setAudioMode(m_soundMode);
        DEBUG_STREAM_OUT("can:rsetAudioMode");
        return;
    }
    else
    {
        // #2 IO扬声器
        ioSpeaker();
    }
}

bool IoControl::soundLightIoSet(std::string mode, const MapIterType &ioIter)
{
    if (ioIter == light_sound_devices.end())
    {
        DEBUG_ERROR_OUT("error: iter is end soundLightIoSet mode=" << mode);
        return false;
    }

    SetFrequency io;
    io.name = ioIter->first;
    io.index = ioIter->second;

    if (io.index < 0 || io.index >= (int)(m_ioGroup.polarity_output.size()))
    {
        DEBUG_STREAM_OUT("io.index error name:" << io.name << ",io.index=" << io.index);
        return false;
    }

    if (mode == "lamp")
    {
        // 可能带频率，可能不带频率
        io.frequency = m_lamp.set_frequency_;
        io.level = m_lamp.level_;
        io.on_time = m_lamp.on_time_;
        io.off_time = m_lamp.off_time_;
        // add at 2023-12-6
        io.level = m_ioGroup.polarity_output[io.index] ? io.level : !io.level;
    }
    else if (mode == "speaker")
    {
        // 不带频率
        io.frequency = false;
        io.level = true;
        io.on_time = 0;
        io.off_time = 0;
        io.level = m_ioGroup.polarity_output[io.index] ? io.level : !io.level;
    }
    else if (mode == "close")
    {
        // 不带频率
        io.frequency = false;
        io.level = false;
        io.on_time = 0;
        io.off_time = 0;
        io.level = m_ioGroup.polarity_output[io.index] ? io.level : !io.level;
    }

    DEBUG_OUT("mode=%s, name=%s, index=%d, frequency=%d, level=%d, on=%d, off=%d",
             mode.c_str(), io.name.c_str(), io.index,
             io.frequency, io.level, io.on_time, io.off_time);

    return io_interface->set_interval_reg(io);
}

//
// 灯光输出
//
void IoControl::light()
{
    try
    {
        // FIXME: test double steer special light
        MapIterType deviceIter;
        // 是否开启绿灯
        /* rewrite
        if (std::string(getenv("ROBOT")).compare("double_steer") == 0 && getenv("VEHICLE") && std::string(getenv("VEHICLE")).compare("_beiben_3T") == 0)
        {
            // 是否开启绿灯
            deviceIter = light_sound_devices.find("leftGreenLight");
            soundLightIoSet((m_lamp.color["green"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rightGreenLight");
            soundLightIoSet((m_lamp.color["green"] ? "lamp" : "close"), deviceIter);

            // 是否开启黄灯
            deviceIter = light_sound_devices.find("leftYellowLight");
            soundLightIoSet((m_lamp.color["yellow"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rightYellowLight");
            soundLightIoSet((m_lamp.color["yellow"] ? "lamp" : "close"), deviceIter);

            // 是否开启红灯
            deviceIter = light_sound_devices.find("leftRedLight");
            soundLightIoSet((m_lamp.color["red"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rightRedLight");
            soundLightIoSet((m_lamp.color["red"] ? "lamp" : "close"), deviceIter);
        }
        if (std::string(getenv("ROBOT")).compare("amr") == 0 && getenv("VEHICLE") && std::string(getenv("VEHICLE")).compare("_newstart") == 0)
        {
            // 是否开启绿灯
            deviceIter = light_sound_devices.find("frontLeftGreenLight");
            soundLightIoSet((m_lamp.color["green"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("frontRightGreenLight");
            soundLightIoSet((m_lamp.color["green"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rearLeftGreenLight");
            soundLightIoSet((m_lamp.color["green"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rearRightGreenLight");
            soundLightIoSet((m_lamp.color["green"] ? "lamp" : "close"), deviceIter);

            // 是否开启黄灯
            deviceIter = light_sound_devices.find("frontLeftYellowLight");
            soundLightIoSet((m_lamp.color["yellow"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("frontRightYellowLight");
            soundLightIoSet((m_lamp.color["yellow"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rearLeftYellowLight");
            soundLightIoSet((m_lamp.color["yellow"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rearRightYellowLight");
            soundLightIoSet((m_lamp.color["yellow"] ? "lamp" : "close"), deviceIter);

            // 是否开启红灯
            deviceIter = light_sound_devices.find("frontLeftRedLight");
            soundLightIoSet((m_lamp.color["red"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("frontRightRedLight");
            soundLightIoSet((m_lamp.color["red"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rearLeftRedLight");
            soundLightIoSet((m_lamp.color["red"] ? "lamp" : "close"), deviceIter);
            deviceIter = light_sound_devices.find("rearRightRedLight");
            soundLightIoSet((m_lamp.color["red"] ? "lamp" : "close"), deviceIter);
        }

        else
        {
            deviceIter = light_sound_devices.find("greenLight");
            soundLightIoSet((m_lamp.color["green"] ? "lamp" : "close"), deviceIter);

            // 是否开启黄灯m_curEvent
            deviceIter = light_sound_devices.find("yellowLight");
            soundLightIoSet((m_lamp.color["yellow"] ? "lamp" : "close"), deviceIter);

            // 是否开启红灯
            deviceIter = light_sound_devices.find("redLight");
            soundLightIoSet((m_lamp.color["red"] ? "lamp" : "close"), deviceIter);
        }
        */

        deviceIter = light_sound_devices.find("greenLight");
        soundLightIoSet((m_lamp.color["green"] ? "lamp" : "close"), deviceIter);

        // 是否开启黄灯m_curEvent
        deviceIter = light_sound_devices.find("yellowLight");
        soundLightIoSet((m_lamp.color["yellow"] ? "lamp" : "close"), deviceIter);

        // 是否开启红灯
        deviceIter = light_sound_devices.find("redLight");
        soundLightIoSet((m_lamp.color["red"] ? "lamp" : "close"), deviceIter);
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what() << '\n';
        DEBUG_ERROR_OUT("error:light crash:" << std::string(e.what()));
    }
    catch (...)
    {
        DEBUG_ERROR_OUT("error:light other crash:");
    }
}

} // end namespace